const int AIN1 = 5;
const int AIN2 = 4;
const int PWMA = 3;

const int BIN1 = 6;
const int BIN2 = 7;
const int PWMB = 9;

const int STBY = 8;

const int delayPaso = 5;

// Calibración inicial
const int pasosPorMM = 100;
const int carreraMM = 11;
const int pasosCarreraTotal = pasosPorMM * carreraMM;

const int secuencia[4][4] = {
  { 1, 0, 1, 0 },
  { 0, 1, 1, 0 },
  { 0, 1, 0, 1 },
  { 1, 0, 0, 1 }
};

int pasoActual = 0;
long posicionPasos = 0;
long posicionMaxima = pasosCarreraTotal;

// Control de movimiento no bloqueante
bool motorEnMovimiento = false;
int direccionMovimiento = 0;  // 1 = adelante, -1 = atrás
long pasosRestantes = 0;

unsigned long ultimoPasoTiempo = 0;

void setup() {
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);

  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);

  pinMode(STBY, OUTPUT);

  digitalWrite(STBY, HIGH);
  digitalWrite(PWMA, HIGH);
  digitalWrite(PWMB, HIGH);

  apagarMotor();

  Serial.begin(115200);
  delay(1000);

  Serial.println("Arduino Nano listo");
  Serial.println("Comandos:");
  Serial.println("f = avanzar 1 mm");
  Serial.println("b = retroceder 1 mm");
  Serial.println("o = avanzar hasta el maximo");
  Serial.println("c = regresar a cero");
  Serial.println("s = stop / pausa");
  Serial.println("p = mostrar posicion");
}

void loop() {
  leerComandosSerial();
  actualizarMovimiento();
}

void leerComandosSerial() {
  if (Serial.available() > 0) {
    char comando = Serial.read();

    if (comando == 'f' || comando == 'F') {
      iniciarMovimientoLimitado(pasosPorMM);
      Serial.println("Avanzando 1 mm");
    } else if (comando == 'b' || comando == 'B') {
      iniciarMovimientoLimitado(-pasosPorMM);
      Serial.println("Retrocediendo 1 mm");
    } else if (comando == 'o' || comando == 'O') {
      long pasosHastaMax = posicionMaxima - posicionPasos;
      iniciarMovimientoLimitado(pasosHastaMax);
      Serial.println("Moviendose hacia adelante hasta el maximo");
    } else if (comando == 'c' || comando == 'C') {
      long pasosHastaCero = -posicionPasos;
      iniciarMovimientoLimitado(pasosHastaCero);
      Serial.println("Moviendose hacia atras hasta cero");
    } else if (comando == 's' || comando == 'S') {
      motorEnMovimiento = false;
      pasosRestantes = 0;
      apagarMotor();
      Serial.println("Movimiento pausado");
    } else if (comando == 'p' || comando == 'P') {
      imprimirPosicion();
    }
  }
}

void iniciarMovimientoLimitado(long pasos) {
  long destino = posicionPasos + pasos;

  if (destino > posicionMaxima) {
    pasos = posicionMaxima - posicionPasos;
  }

  if (destino < 0) {
    pasos = -posicionPasos;
  }

  if (pasos == 0) {
    motorEnMovimiento = false;
    pasosRestantes = 0;
    apagarMotor();
    Serial.println("Ya esta en el limite");
    return;
  }

  direccionMovimiento = (pasos > 0) ? 1 : -1;
  pasosRestantes = abs(pasos);
  motorEnMovimiento = true;
}

void actualizarMovimiento() {
  if (!motorEnMovimiento) return;

  unsigned long tiempoActual = millis();

  if (tiempoActual - ultimoPasoTiempo >= delayPaso) {
    ultimoPasoTiempo = tiempoActual;

    // Verificar límites antes de dar el paso
    if (direccionMovimiento == 1 && posicionPasos >= posicionMaxima) {
      motorEnMovimiento = false;
      pasosRestantes = 0;
      apagarMotor();
      Serial.println("Limite maximo alcanzado");
      imprimirPosicion();
      return;
    }

    if (direccionMovimiento == -1 && posicionPasos <= 0) {
      motorEnMovimiento = false;
      pasosRestantes = 0;
      apagarMotor();
      Serial.println("Posicion cero alcanzada");
      imprimirPosicion();
      return;
    }

    darUnPaso(direccionMovimiento);
    posicionPasos += direccionMovimiento;
    pasosRestantes--;

    if (pasosRestantes <= 0) {
      motorEnMovimiento = false;
      apagarMotor();
      Serial.println("Movimiento completado");
      imprimirPosicion();
    }
  }
}

void darUnPaso(int direccion) {
  pasoActual += direccion;

  if (pasoActual > 3) pasoActual = 0;
  if (pasoActual < 0) pasoActual = 3;

  setStep(secuencia[pasoActual]);
}

void setStep(const int paso[4]) {
  digitalWrite(AIN1, paso[0]);
  digitalWrite(AIN2, paso[1]);
  digitalWrite(BIN1, paso[2]);
  digitalWrite(BIN2, paso[3]);
}

void apagarMotor() {
  digitalWrite(AIN1, LOW);
  digitalWrite(AIN2, LOW);
  digitalWrite(BIN1, LOW);
  digitalWrite(BIN2, LOW);
}

void imprimirPosicion() {
  Serial.print("Posicion estimada: ");
  Serial.print((float)posicionPasos / pasosPorMM);
  Serial.println(" mm");
}